#include "aastar.h"
#include <cstdlib>
#include <cassert>
#include <string>

#include "utils.h"

MapZone::MapZone() :
    _width(0),
    _height(0),
    _map(nullptr)
{

}

MapZone::~MapZone()
{
    if (_map)
    {
        free(_map);
        _map = nullptr;
    }
}

bool MapZone::init( size_t width, size_t height )
{
    _width = width;
    _height = height;

    // map should be non initialized
    assert(_map == nullptr);

    uint32_t width2p = clp2(_width);
    _logWidth = log2(width2p);

    size_t size = sizeof( bool ) * width2p * _height;
    _map = (bool*)malloc( size );
    memset(_map, 0xff, size);
    return true;
}

bool MapZone::isPassable( size_t x, size_t y) const
{
    assert(x < _width);
    assert(y < _height);

    return _map[ getIndexForPoint(y, x) ];
}

void MapZone::setPassable( size_t x, size_t y, bool passable ) const
{
    _map[ getIndexForPoint(x, y) ] = passable;
}

size_t MapZone::getIndexForPoint( size_t y, size_t x ) const
{
    return static_cast<uint32_t>(y << _logWidth) + x;
}

bool MapZone::isPointInside( int x, int y ) const
{
    return x >= 0 && y >= 0 && x < _width && y < _height;
}

bool MapZone::findPath( const MapTile& start, const MapTile& end, vector<MapTile>* path )
{
    AStarSearch<MapZone::Node> astarsearch;

    MapZone::Node nodeStart(this, start.x, start.y);

    // Define the goal state
    MapZone::Node nodeEnd(this, end.x, end.y);

    // Set Start and goal states
    astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd );

    unsigned int SearchState;

    unsigned int SearchSteps = 0;
    path->clear();
    do
    {
        SearchState = astarsearch.SearchStep();

        SearchSteps++;
    }
    while( SearchState == AStarSearch<MapZone::Node>::SEARCH_STATE_SEARCHING );

    if( SearchState == AStarSearch<MapZone::Node>::SEARCH_STATE_SUCCEEDED )
    {

        MapZone::Node *node = astarsearch.GetSolutionStart();
        int steps = 0;

        for( ;; )
        {
            node = astarsearch.GetSolutionNext();
           

            if( !node )
            {
                break;
            }

            path->push_back(node->getTile());

            steps ++;

        };

        node = astarsearch.GetSolutionEnd();
    }
    return !path->empty();
}
